#include "TerritorialityModule.h"
#include "AttractivenessMap.h"
#include "OccupancyGrid.h"

#include <cstdlib>
#include <vector>
using namespace std;

TerritorialityModule* TerritorialityModule::_instance=0;

TerritorialityModule::~TerritorialityModule()
{

}

TerritorialityModule* TerritorialityModule::instance()
{
	if(_instance == 0)
		_instance = new TerritorialityModule();
	return _instance;
}

int chooseOptionWithProbs(vector<double>& probs)
{
    int n=probs.size();
    int maxProb=0, maxProbIndex=0;
    double probsSum=0;
    for(int i=0;i<n;i++)
    {
    	if(probs[i]>maxProb)
    	{
    		maxProb=probs[i];
    		maxProbIndex=i;
    	}
        probsSum+=probs[i];
        probs[i]=probsSum;
    }
    double r = ((double) rand() / (RAND_MAX))*probsSum;
    for(int i=0;i<n;i++)
    {
        if(r<=probs[i])
        {
            return i;
        }
    }
    return maxProbIndex;
}

Tile TerritorialityModule::getDecision()
{

    Tile t = Controller::instance()->getTile();
    int x = t.x, y=t.y;
    printf("main tile coords: %d,%d\n",x,y);
    vector<AttractivenessMap::tpair > attr;
    attr= AttractivenessMap::instance()->getNeighbors(x,y);

    int n=attr.size();
    printf("n=%d\n",n);
    bool allZero=true;
    for(int i=0;i<n;i++)
    {
    	printf("n tile coords:%d,%d\n",attr[i].first.x,attr[i].first.y);
        if(OccupancyGrid::instance()->at(attr[i].first.x,attr[i].first.y)<=0.5)
        {
            double neighSum=0;
            vector<pair<Tile, double> > nnAttr = AttractivenessMap::instance()->getNeighbors(attr[i].first.x,attr[i].first.y);
            int nn=nnAttr.size();
            printf("nnSize=%d\n",nn);

            for(int j=0;j<nn;j++)
            {
            	printf("nn tile coords:%d,%d\n",nnAttr[j].first.x,nnAttr[j].first.y);
                Tile tile = nnAttr[j].first;
                if(OccupancyGrid::instance()->at(tile.x,tile.y)<=0.5)
                {
                    neighSum+=AttractivenessMap::instance()->at(tile.x,tile.y);
                }
                else
                {
                    nnAttr.erase(nnAttr.begin()+j);
                    nn--;
                    j--;
                }
                printf("end of j\n");
            }
            attr[i].second/=neighSum;
            if(attr[i].second!=0)
            {
                allZero=false;
            }
        }
        else
        {
            attr.erase(attr.begin()+i);
            n--;
            i--;
        }
    }

    vector<double> probs(n,0.0);
    probs.resize(n);
    if(allZero)
    {
        printf("allzero\n");
        for(int i=0;i<n;i++)
        {
            probs[i]=1.0/n;
            printf("probs[%d]=%lf\n",i,probs[i]);
        }
    }
    else
    {
    	printf("nonzero\n");
        for(int i=0;i<n;i++)
        {
            probs[i]=attr[i].second;
            printf("probs[%d]=%lf\n",i,probs[i]);

        }
    }
    int chosen=chooseOptionWithProbs(probs);
    printf("chosen=%d\n",chosen);
    return attr[chosen].first;
}
